#include "DepthInputFilter.h"
#include <QDebug>
#include <QString>

DepthInputFilter::DepthInputFilter(int width, int height, int fps, bool mirror) : width(width), height(height), InputFilter()
{
	mapMode.nXRes = width;
	mapMode.nYRes = height;
	mapMode.nFPS = fps;

	node = &depth;
	XnStatus status = depth.Create(context);
	depth.SetMapOutputMode(mapMode);
	depth.GetMirrorCap().SetMirror(mirror);

	outputs[0] = cv::Mat(cv::Size(width, height), CV_16UC1);

	context.StartGeneratingAll();
}


DepthInputFilter::~DepthInputFilter(void)
{
}

void DepthInputFilter::Process()
{
	std::memcpy(outputs[0].data, depth.GetDepthMap(), width*height*sizeof(ushort));
}

cv::Mat DepthInputFilter::ConvertProjectiveToRealWorld(cv::Mat projective)
{
	XnPoint3D *points = new XnPoint3D[projective.rows];
	XnPoint3D *result = new XnPoint3D[projective.rows];

	for (int i = 0; i < projective.rows; ++i)
	{
		float *p = projective.ptr<float>(i);
		points[i].X = p[0];
		points[i].Y = p[1];
		points[i].Z = p[2];
	}

	depth.ConvertProjectiveToRealWorld(projective.rows, points, result);
	cv::Mat realWorld(projective.rows, 3, CV_32FC1);
	
	for (int i = 0; i < projective.rows; ++i)
	{
		float *r = realWorld.ptr<float>(i);
		r[0] = result[i].X;
		r[1] = result[i].Y;
		r[2] = result[i].Z;
	}

	delete[] points;
	delete[] result;

	return realWorld;
}

cv::Mat DepthInputFilter::CovertRealWorldToProjective(cv::Mat realWorld)
{
	XnPoint3D *points = new XnPoint3D[realWorld.rows];
	XnPoint3D *result = new XnPoint3D[realWorld.rows];

	for (int i = 0; i < realWorld.rows; ++i)
	{
		float *p = realWorld.ptr<float>(i);
		points[i].X = p[0];
		points[i].Y = p[1];
		points[i].Z = p[2];
	}

	depth.ConvertRealWorldToProjective(realWorld.rows, points, result);

	cv::Mat projective(realWorld.rows, 3, CV_32FC1);

	for (int i = 0; i < realWorld.rows; ++i)
	{
		float *r = projective.ptr<float>(i);
		r[0] = result[i].X;
		r[1] = result[i].Y;
		r[2] = result[i].Z;
	}

	delete[] points;
	delete[] result;

	return projective;
}
